/* Astar implementation
 *
 * Astar implementation, currently implementing using a heap provided by
 * the standard stl heap functions in algorithm.
 *
 * By: M. Etheredge
 *     S. Brattinga
 */

#ifndef ASTAR_H
#define ASTAR_H

#include <math.h>
#include <time.h>
#include <stdlib.h>

#include <vector>
#include <list>
#include <map>
#include <algorithm>
#include <iostream>
#include <iterator>

namespace Astar
{

    #define STATE_UNINITIALIZED_MAP 0
    #define STATE_INITIALIZED_MAP   1

    #define ADJACENT_POSITION_TOP_LEFT  0
    #define ADJACENT_POSITION_TOP       1
    #define ADJACENT_POSITION_TOP_RIGHT 2
    #define ADJACENT_POSITION_LEFT      3
    #define ADJACENT_POSITION_RIGHT     4
    #define ADJACENT_POSITION_BOT_LEFT  5
    #define ADJACENT_POSITION_BOT       6
    #define ADJACENT_POSITION_BOT_RIGHT 7

    class Node
    {
    private:
        float m_Cost;
        float m_Distance;
        float m_CumulativeCost;
        bool  m_Obstacle;

        int m_X;
        int m_Y;

        int m_Height;
    public:
        Node( const int p_X, const int p_Y )
        {
            m_X = p_X;
            m_Y = p_Y;
            m_Height = 0;
        }

        float GetCost           ( void ) { return m_Cost; }           void SetCost           ( const float p_Cost )           { m_Cost = p_Cost; }
        float GetDistance       ( void ) { return m_Distance; }       void SetDistance       ( const float p_Distance )       { m_Distance = p_Distance; }
        float GetCumulativeCost ( void ) { return m_CumulativeCost; } void SetCumulativeCost ( const float p_CumulativeCost ) { m_CumulativeCost = p_CumulativeCost; }
        int   GetX              ( void ) { return m_X; }              void SetX              ( const int p_X )                { m_X = p_X; }
        int   GetY              ( void ) { return m_Y; }              void SetY              ( const int p_Y )                { m_Y = p_Y; }
        int   GetHeight         ( void ) { return m_Height; }         void SetHeight         ( const int p_Height )           { m_Height = p_Height; }
        bool  GetObstacle       ( void ) { return m_Obstacle; }       void SetObstacle       ( const bool p_Obstacle )        { m_Obstacle = p_Obstacle; }

        bool operator==( Node& p_Node )
		{
			return GetX() == p_Node.GetX() && GetY() == p_Node.GetY();
		}
    };

    class HeapCompareCost
    {
    public:
        bool operator() ( Node *p_A, Node *p_B ) const
        {
            return p_A->GetCumulativeCost() > p_B->GetCumulativeCost();
        }
    };

    extern Node ***g_Map;

    extern int g_Width;
    extern int g_Height;

    extern int g_State;

    extern Node *g_AdjacentNodes[8];

    // Assumes a sorted list (by sorted is meant, top left at front, bottom right at back),
    // currently this is just a simple vector to array converter, but in case we might want to
    // change the map structure for some reason...
    void InitializeMap( std::vector<Node*> &p_Nodes, const int p_Width );

    void SetAdjacentNodeIndices( Node *p_Node );

    void FreeMap( void );

    std::list<Node*> Astar( Node *p_Start, Node *p_Goal );

};

namespace Heuristics
{

    int Score( const int p_MinimumCost, Astar::Node *p_Goal, Astar::Node *p_Current );
    int ManhattanDistance( const int p_MinimumCost, Astar::Node *p_Goal, Astar::Node *p_Current );

};

#endif
